/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package ticks.PID.commands;

/**
 *
 * @author Jared
 */
public class ArmSetpoint extends CommandBase {
    public static double Scoring = 0.7; //Some preset positions--NOTE THE STATIC KEYWORD!!!
    public static double Stowed = 0.2;
    double setpoint;
    public ArmSetpoint(double setpoint) {//This argument determines the setpoint of the arm
        requires(CommandBase.armPid);
        this.setpoint = setpoint;  
        
        //makes the argument setpoint = to the variable setpoint
        //will interrupt the default operator control command
        // Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);
        setTimeout(3); //An added feature which will abort the command if the arm takes more than 3 sec
        //to reach desired position
    }

    // Called just before this Command runs the first time
    protected void initialize() {
        armPid.setArmSetpoint(setpoint);
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
        
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
        return isTimedOut()||armPid.armAtSetpoint();
        //will cause the command to terminate if
            //The arm takes too long
            //OR
            //The arm reaches the desired postion
    }

    // Called once after isFinished returns true
    protected void end() {
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
    }
}
